/*
 * This file is part of the Sx Framework Library.
 * 
 * Copyright (C) 2013 University of Colorado Denver
 * <min.choi@ucdenver.edu> <shane.transue@ucdenver.edu>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy 
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
 * copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 */
#include <sxDistances.h>

float Sx::Math::Distances::PointToSegmentDistance(const Eigen::Vector3f& point, const Eigen::Vector3f& segment_p0, const Eigen::Vector3f& segment_p1) {
	Eigen::Vector3f v = segment_p1 - segment_p0;
	Eigen::Vector3f w = point - segment_p0;

	double c1 = w.dot(v);
	if ( c1 <= 0 ) return Math::Distance(point, segment_p0);

	double c2 = v.dot(v);
	if ( c2 <= c1 ) return Math::Distance(point, segment_p1);

	double b = c1 / c2;
	Eigen::Vector3f Pb = segment_p0 + static_cast<float>(b) * v;
	return Math::Distance(point, Pb);
}

Eigen::Vector3d Sx::Math::Distances::ClosestPointOnSegment(const Eigen::Vector3d& segment_p0, const Eigen::Vector3d& segment_p1, const Eigen::Vector3d& point) {
	Eigen::Vector3d v = segment_p1 - segment_p0;
	Eigen::Vector3d w = point - segment_p0;

	double c1 = w.dot(v);
	if ( c1 <= 0 ) return segment_p0;

	double c2 = v.dot(v);
	if ( c2 <= c1 ) return segment_p1;

	double b = c1 / c2;
	Eigen::Vector3d Pb = segment_p0 + b * v;
	return Pb;
}

Eigen::Vector3d Sx::Math::Distances::ClosestPointOnLine(const Eigen::Vector3d& segment_p0, const Eigen::Vector3d& segment_p1, const Eigen::Vector3d& point) {
	Eigen::Vector3d v = segment_p1 - segment_p0;
	Eigen::Vector3d w = point - segment_p0;

	double c1 = w.dot(v);
	double c2 = v.dot(v);
	double b = c1 / c2;
	Eigen::Vector3d Pb = segment_p0 + b * v;
	return Pb;
}

Eigen::Vector3f Sx::Math::Distances::PointToSegmentPerpendicularDirection(const Eigen::Vector3f& point, const Eigen::Vector3f segment_p0, const Eigen::Vector3f segment_p1) {
	Eigen::Vector3f v = segment_p1 - segment_p0;
	Eigen::Vector3f w = point - segment_p0;

	double c1 = w.dot(v);
	if ( c1 <= 0 ) return (segment_p0 - point).normalized();

	double c2 = v.dot(v);
	if ( c2 <= c1 ) return (segment_p1 - point).normalized();

	double b = c1 / c2;
	Eigen::Vector3f Pb = segment_p0 + static_cast<float>(b) * v;
	return (Pb - point).normalized();
}

double Sx::Math::Distances::SegmentToSegmentDistance(const Eigen::Vector3d& segment_1_p0, const Eigen::Vector3d& segment_1_p1, const Eigen::Vector3d& segment_2_p0, const Eigen::Vector3d& segment_2_p1) {
	Eigen::Vector3d u = segment_1_p1 - segment_1_p0;
    Eigen::Vector3d v = segment_2_p1 - segment_2_p0;
    Eigen::Vector3d w = segment_1_p0 - segment_2_p0;

    double a = u.dot(u);
    double b = u.dot(v);
    double c = v.dot(v);
    double d = u.dot(w);
    double e = v.dot(w);

    double D = a*c - b*b;
    double sc, sN, sD = D;
    double tc, tN, tD = D;

    if ( D < std::numeric_limits<double>::epsilon() ) {
        sN = 0.0;
        sD = 1.0;
        tN = e;
        tD = c;
    }
    else {
        sN = (b*e - c*d);
        tN = (a*e - b*d);
        if (sN < 0.0) {
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD) {
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0) {
        tN = 0.0;

        if (-d < 0.0)
            sN = 0.0;
        else if (-d > a)
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) {
        tN = tD;

        if ((-d + b) < 0.0)
            sN = 0;
        else if ((-d + b) > a)
            sN = sD;
        else {
            sN = (-d + b);
            sD = a;
        }
    }

    sc = (abs(sN) < std::numeric_limits<double>::epsilon() ? 0.0 : sN / sD);
    tc = (abs(tN) < std::numeric_limits<double>::epsilon() ? 0.0 : tN / tD);

    Eigen::Vector3d  dP = w + (u * sc) - (v * tc);

    return sqrt(dP.dot(dP));
}

float Sx::Math::Distances::SegmentToSegmentDistance(const Eigen::Vector3f& segment_1_p0, const Eigen::Vector3f& segment_1_p1, const Eigen::Vector3f& segment_2_p0, const Eigen::Vector3f& segment_2_p1) {
	Eigen::Vector3f u = segment_1_p1 - segment_1_p0;
    Eigen::Vector3f v = segment_2_p1 - segment_2_p0;
    Eigen::Vector3f w = segment_1_p0 - segment_2_p0;

    float a = static_cast<float>(u.dot(u));
    float b = static_cast<float>(u.dot(v));
    float c = static_cast<float>(v.dot(v));
    float d = static_cast<float>(u.dot(w));
    float e = static_cast<float>(v.dot(w));

    float D = a*c - b*b;
    float sc, sN, sD = D;
    float tc, tN, tD = D;

    if ( D < std::numeric_limits<float>::epsilon() ) {
        sN = 0.0;
        sD = 1.0;
        tN = e;
        tD = c;
    }
    else {
        sN = (b*e - c*d);
        tN = (a*e - b*d);
        if (sN < 0.0) {
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD) {
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0) {
        tN = 0.0;

        if (-d < 0.0)
            sN = 0.0;
        else if (-d > a)
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) {
        tN = tD;

        if ((-d + b) < 0.0)
            sN = 0;
        else if ((-d + b) > a)
            sN = sD;
        else {
            sN = (-d + b);
            sD = a;
        }
    }

    sc = (abs(sN) < std::numeric_limits<float>::epsilon() ? 0.0f : sN / sD);
    tc = (abs(tN) < std::numeric_limits<float>::epsilon() ? 0.0f : tN / tD);

    Eigen::Vector3f  dP = w + (u * sc) - (v * tc);

    return static_cast<float>(sqrt(dP.dot(dP)));
}

inline double clamp(double x, double a, double b) {
    return x < a ? a : (x > b ? b : x);
}

Eigen::Vector3d Sx::Math::Distances::closesPointOnTriangle(const Eigen::Vector3d& triangle_Point0, const Eigen::Vector3d& triangle_Point1, const Eigen::Vector3d& triangle_Point2, const Eigen::Vector3d& point) {
    Eigen::Vector3d  edge0 = triangle_Point1 - triangle_Point0;
    Eigen::Vector3d  edge1 = triangle_Point2 - triangle_Point0;
    Eigen::Vector3d  v0 = triangle_Point0 - point;

    double a = edge0.dot( edge0 );
    double b = edge0.dot( edge1 );
    double c = edge1.dot( edge1 );
    double d = edge0.dot( v0 );
    double e = edge1.dot( v0 );

    double det = a*c - b*b;
    double s = b*e - c*d;
    double t = b*d - a*e;

    if ( s + t < det )
    {
        if ( s < 0.0 )
        {
            if ( t < 0.0 )
            {
                if ( d < 0.0 )
                {
                    s = clamp( -d/a, 0.0, 1.0 );
                    t = 0.0;
                }
                else
                {
                    s = 0.0;
                    t = clamp( -e/c, 0.0, 1.0 );
                }
            }
            else
            {
                s = 0.0;
                t = clamp( -e/c, 0.0, 1.0 );
            }
        }
        else if ( t < 0.0 )
        {
            s = clamp( -d/a, 0.0, 1.0 );
            t = 0.0;
        }
        else
        {
            double invDet = 1.0 / det;
            s *= invDet;
            t *= invDet;
        }
    }
    else
    {
        if ( s < 0.0 )
        {
            double tmp0 = b+d;
            double tmp1 = c+e;
            if ( tmp1 > tmp0 )
            {
                double numer = tmp1 - tmp0;
                double denom = a-2*b+c;
                s = clamp( numer/denom, 0.0, 1.0 );
                t = 1-s;
            }
            else
            {
                t = clamp( -e/c, 0.0, 1.0 );
                s = 0.0;
            }
        }
        else if ( t < 0.0 )
        {
            if ( a+d > b+e )
            {
                double numer = c+e-b-d;
                double denom = a-2*b+c;
                s = clamp( numer/denom, 0.0, 1.0 );
                t = 1-s;
            }
            else
            {
                s = clamp( -e/c, 0.0, 1.0 );
                t = 0.0;
            }
        }
        else
        {
            double numer = c+e-b-d;
            double denom = a-2*b+c;
            s = clamp( numer/denom, 0.0, 1.0 );
            t = 1.0 - s;
        }
    }

    return triangle_Point0 + s * edge0 + t * edge1;
}

double Sx::Math::Distances::PointToPlaneDistance(const Eigen::Vector3d& planeNormal, const Eigen::Vector3d& planePoint, const Eigen::Vector3d& point, Eigen::Vector3d& pointOnPlane) {
	double sb, sn, sd;
	Eigen::Vector3d pplv0 = point - planePoint;
	sn = -planeNormal.dot(pplv0);
	sd = planeNormal.dot(planeNormal);
	if ( sd == 0.0 ) return 0.0;
	sb = sn / sd;

	pointOnPlane = point + sb * planeNormal;
	return Distance(point, pointOnPlane);
}
